#include "industrial_robot_control/ethercat/master.h"
#include <ethercat.h>
#include <thread>
#include <iostream>
#include "industrial_robot_control/utils/log.h"

namespace IndustrialRobotControl {
namespace Ethercat {

Master::Master() : is_initialized_(false), ecx_context_(nullptr) {}

Master::~Master() {
    shutdown();
}

bool Master::initialize(const YAML::Node& config) {
    LOG_INFO("Initializing EtherCAT master");
    
    // 检查配置
    if (!config["ifname"]) {
        LOG_ERROR("EtherCAT interface name not specified in config");
        return false;
    }
    std::string ifname = config["ifname"].as<std::string>();
    
    // 初始化EtherCAT库
    ecx_context_ = ecx_init(ifname.c_str());
    if (!ecx_context_) {
        LOG_ERROR("Failed to initialize EtherCAT on interface: {}", ifname);
        return false;
    }
    
    // 配置从站数量
    num_slaves_ = config["num_slaves"].as<int>(1);
    LOG_INFO("Configured for {} EtherCAT slaves", num_slaves_);
    
    // 初始化从站
    if (!initialize_slaves()) {
        ecx_close(ecx_context_);
        ecx_context_ = nullptr;
        return false;
    }
    
    // 启动EtherCAT通信
    if (ecx_statecheck(ecx_context_, 0, EC_STATE_OPERATIONAL, 1000) != EC_STATE_OPERATIONAL) {
        LOG_ERROR("Failed to set EtherCAT master to operational state");
        ecx_close(ecx_context_);
        ecx_context_ = nullptr;
        return false;
    }
    
    is_initialized_ = true;
    LOG_INFO("EtherCAT master initialized successfully");
    return true;
}

void Master::shutdown() {
    if (is_initialized_) {
        LOG_INFO("Shutting down EtherCAT master");
        
        // 停止所有轴
        stop_all_axes();
        
        // 关闭EtherCAT
        if (ecx_context_) {
            ecx_close(ecx_context_);
            ecx_context_ = nullptr;
        }
        
        is_initialized_ = false;
    }
}

bool Master::initialize_slaves() {
    // 查找并配置所有从站
    for (int i = 1; i <= num_slaves_; ++i) {
        if (ecx_statecheck(ecx_context_, i, EC_STATE_PREOP, 1000) != EC_STATE_PREOP) {
            LOG_ERROR("Slave {} not in PREOP state", i);
            return false;
        }
        
        // 配置从站为OPERATIONAL状态
        ecx_slave_config(ecx_context_, i, nullptr);
        
        if (ecx_statecheck(ecx_context_, i, EC_STATE_OPERATIONAL, 1000) != EC_STATE_OPERATIONAL) {
            LOG_ERROR("Failed to set slave {} to operational state", i);
            return false;
        }
        
        LOG_INFO("Slave {} initialized successfully", i);
    }
    
    return true;
}

void Master::read_states() {
    if (!is_initialized_) return;
    
    // 读取从站数据
    ecx_sdo_read(ecx_context_, 1, 0x6064, 0x00, sizeof(int32_t), &current_position_);
    ecx_sdo_read(ecx_context_, 1, 0x606C, 0x00, sizeof(int32_t), &current_velocity_);
    ecx_sdo_read(ecx_context_, 1, 0x6078, 0x00, sizeof(int16_t), &current_torque_);
    
    // 读取状态字
    ecx_sdo_read(ecx_context_, 1, 0x6041, 0x00, sizeof(uint16_t), &status_word_);
}

void Master::write_commands() {
    if (!is_initialized_) return;
    
    // 写入控制字
    ecx_sdo_write(ecx_context_, 1, 0x6040, 0x00, sizeof(uint16_t), &control_word_);
    
    // 写入目标位置（如果有）
    if (has_target_position_) {
        ecx_sdo_write(ecx_context_, 1, 0x607A, 0x00, sizeof(int32_t), &target_position_);
        has_target_position_ = false;
    }
}

std::vector<double> Master::get_joint_positions() {
    // 转换原始位置数据为角度（示例）
    std::vector<double> positions;
    positions.push_back(static_cast<double>(current_position_) * 0.001); // 假设转换因子
    return positions;
}

void Master::set_joint_targets(const std::vector<double>& targets) {
    if (targets.empty()) return;
    
    // 转换角度为原始位置数据（示例）
    target_position_ = static_cast<int32_t>(targets[0] * 1000); // 假设转换因子
    has_target_position_ = true;
    
    // 设置控制字启动运动
    control_word_ = 0x000F; // 具体值需参考从站手册
}

void Master::stop_all_axes() {
    if (!is_initialized_) return;
    
    // 发送停止命令
    control_word_ = 0x0006; // 具体值需参考从站手册
    ecx_sdo_write(ecx_context_, 1, 0x6040, 0x00, sizeof(uint16_t), &control_word_);
    
    // 等待停止
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

} // namespace Ethercat
} // namespace IndustrialRobotControl
